
from maya.OpenMaya import *

import Util
import LocalSpace
import Steering

reload(Util)
reload(LocalSpace)
reload(Steering)

class Vehicle(Steering.Steering):
    serialNumberCounter = 0
    serialNumber = 0
    
    mass = 1
    speed = 0
    radius = 0.5
    maxForce = 0.1
    maxSpeed = 1.0
    
    curvature = 0
    smoothedCurvature = 0.0
    
    lastForward = MVector()
    lastPosition = MVector()
    
    smoothedPosition = MVector()
    smoothedAcceleration = MVector()
    
    def __init__( self ):
        Steering.Steering.__init__( self, self )
        
        self.reset()
        
        self.serialNumber = Vehicle.serialNumberCounter
        Vehicle.serialNumberCounter += 1
    
    def reset( self ):
        self.resetLocalSpace()
        
        Steering.Steering.reset( self )
        
        mass = 1
        speed = 0
        radius = 0.5
        maxForce = 0.1
        maxSpeed = 1.0
        
        self.resetSmoothedPosition()
        self.resetSmoothedCurvature()
        self.resetSmoothedAcceleration()
        
    def resetSmoothedPosition( self, _Vector = MVector.zero ):
        self.smoothedPosition.x = _Vector.x
        self.smoothedPosition.y = _Vector.y
        self.smoothedPosition.z = _Vector.z
        return self.smoothedPosition
        
    def resetSmoothedCurvature( self, _Value = 0 ):
        self.lastForward = MVector.zero
        self.lastPosition = MVector.zero
        self.curvature = _Value
        self.smoothedCurvature = _Value
        return self.smoothedCurvature
        
    def resetSmoothedAcceleration( self, _Vector = MVector.zero ):
        self.smoothedAcceleration.x = _Vector.x
        self.smoothedAcceleration.y = _Vector.y
        self.smoothedAcceleration.z = _Vector.z
        return self.smoothedAcceleration
        
    def velocity( self ): return self.forward * self.speed
    
    def adjustRawSteeringForce( self, _Force, _Dt ):
        _MaxAdjustedSpeed = 0.2 * self.maxSpeed
        if (self.speed > _MaxAdjustedSpeed) or Util.isZeroVector( _Force ):
            return _Force
        else:
            _Range = self.speed / _MaxAdjustedSpeed
            _Cosine = Util.interpolate( pow( _Range, 20 ), 1.0, -1.0 )
            return Util.limitMaxDeviationAngle( _Force, _Cosine, self.forward )
        
    def applySteeringForce( self, _Force, _Dt ):
        _AdjustedForce = self.adjustRawSteeringForce( _Force, _Dt )
        _ClippedForce = Util.truncateLength( _AdjustedForce, self.maxForce )
        
        _NewAcceleration = _ClippedForce / self.mass
        _NewVelocity = self.velocity()
        
        if 0 < _Dt:
            _SmoothRate = Util.clamp( 9 * _Dt, 0.15, 0.5 )
            self.smoothedAcceleration = Util.blendIntoAccumulator( _SmoothRate, _NewAcceleration, self.smoothedAcceleration )
            
        _NewVelocity += self.smoothedAcceleration * _Dt
        _NewVelocity = Util.truncateLength( _NewVelocity, self.maxSpeed )
        
        self.speed = _NewVelocity.length()
        self.position = self.position + ( _NewVelocity * _Dt )
        
        self.regenerateLocalSpace( _NewVelocity, _Dt )
        self.measurePathCurvature( _Dt )
        
        self.smoothedPosition = Util.blendIntoAccumulator( _Dt * 0.06, self.position, self.smoothedPosition )
        
    def applyBrakingForce( _Rate, _Dt ):
        _ClipBraking = _RawBraking = self.speed * _Rate
        if _RawBraking < self.maxForce:
            _ClipBraking = self.maxForce
            
        self.speed = self.speed - (_ClipBraking * _Dt)
    
    def predictNextPosition( self, _PredictionTime ):
        return self.position + (self.velocity * _PredictionTime )
        
    def regenerateLocalSpace( self, _NewVelocity, _Dt ):
        if 0 < self.speed: self.regenerateOrthonormalBasisUF( _NewVelocity / self.speed )
    
    def regenerateLocalSpaceForBanking( self, _NewVelocity, _Dt ):
        _GlobalUp = MVector( 0, 0.2, 0 )
        _AccelUp = self.smoothedAcceleration * 0.05
        _BankUp = _AccelUp + _GlobalUp
        _SmoothRate = _Dt * 3
        _TempUp = self.up
        _TempUp = Util.blendIntoAccumulator( _SmoothRate, _BankUp, _TempUp )
        self.up = _TempUp.normal()
        
        if 0 < self.speed: self.regenerateOrthonormalBasisUF( _NewVelocity / self.speed )
        
    def measurePathCurvature( self, _Dt ):
        if 0 < _Dt:
            _DeltaP = self.lastPosition - self.position
            _DeltaF = (self.lastForward - self.forward) / _DeltaP.length()
            _Lateral = Util.perpendicularComponent( _DeltaF, self.forward )
            _Sign = -1.0
            if _Lateral * self.side: _Sign = 1.0
            
            self.curvature = _Lateral.length() * _Sign
            self.smoothedCurvature = Util.blendIntoAccumulator( _Dt * 4.0, self.curvature, self.smoothedCurvature )
            
            self.lastForward = self.forward
            self.lastPosition = self.position
            
    def randomizeHeadingOnZXPlane( self ):
        self.up = MVector.zero
        self.forward = Util.randomUnitVectorOnXZPlane()
        self.side = self.localRotateForwardToSide( self.forward )